clc
clear 
close all

addpath('../');
addpath('../utils_rotations');
addpath('../utils_KF_DDP');
load problem.mat

N_samples = 100;
rho = 0.1;
relevant_idx = [idx.u_prev idx.u_delta_prev idx.ned_dot idx.ned idx.pqr idx.q];
mu = zeros(1, length(idx.inputs)*length(relevant_idx));
sigma = eye(length(mu));
noise_sigma = 0.4;
evaluate_history = [];

epochs = 1;
while (1) 
    performance = cell(N_samples, 2);

    for i = 1:N_samples
        weight = mvnrnd(mu, sigma);
        t = evaluate_perturb_noise(weight, idx, relevant_idx, target_hover_state, H, dt, model);
        performance{i, 1} = -t;
        performance{i, 2} = weight;
    end

    performance = sortrows(performance,1);

    candidates = [];
    for i = 1:ceil(rho*N_samples)
        candidates = [candidates; performance{i, 2}];
    end

    mu = mean(candidates);
    sigma = cov(candidates);
    sigma = sigma + noise_sigma*sqrt(1/epochs)*eye(size(sigma));

    
    evaluate_t = [];
    N_trials =  30;
    for i = 1:N_trials
        t  = evaluate_perturb_noise(mu, idx, relevant_idx, target_hover_state, H, dt, model);
        evaluate_t = [evaluate_t t];
    end
    mean(evaluate_t)
    evaluate_history = [evaluate_history mean(evaluate_t)];
    epochs = epochs + 1;
end
       